
#include "polyfit.h"

Mat Polyfit(vector<Point>& in_point, int n)
{
	int size = in_point.size();
	//所求未知数个数
	int x_num = n + 1;
	//构造矩阵U和Y
	Mat mat_u(size, x_num, CV_64F);
	Mat mat_y(size, 1, CV_64F);
 
	for (int i = 0; i < mat_u.rows; ++i)
		for (int j = 0; j < mat_u.cols; ++j)
		{
			mat_u.at<double>(i, j) = pow(in_point[i].x, j);
		}
	for (int i = 0; i < mat_y.rows; ++i)
	{
		mat_y.at<double>(i, 0) = in_point[i].y;
	}
	//矩阵运算，获得系数矩阵K
	Mat mat_k(x_num, 1, CV_64F);
	mat_k = (mat_u.t()*mat_u).inv()*mat_u.t()*mat_y;
	cout << mat_k << endl;
	return mat_k;
}

/**
     * 冒泡排序
     *
     * @param array
     * @return
     */
std::vector<cv::Point3d> BubbleSort(std::vector<cv::Point3d> &array) {
	if (array.size() == 0)
		return array;
	for (int i = 0; i < array.size(); i++)
		for (int j = 0; j < array.size() - 1 - i; j++)
			if (array[j + 1].y < array[j].y) {
				cv::Point3d temp = array[j + 1];
				array[j + 1] = array[j];
				array[j] = temp;
			}
	return array;
}

/// y = p1+p2*x + p3*x*x + p4*x*x*x  y 侧向方向  x 车头方向  
LaneParam CalcPolfit3(std::vector<std::pair<double,double>> points)
{
	// int jiange = 10;//相隔十个点选一个点进行计算
	// n = n / jiange + 1;
	//-----------------公式---------------------------
	//n A1     + sumX A2   + sumXX A3   +  sumXXX A4   = sumY
	//sumX A1  + sumXX A2  + sumXXX A3  +  sumXXXX A4  = sumXY
	//sumXX A1 + sumXXX A2 + sumXXXX A3 +  sumXXXXX A4 = sumXXY
	//sumXXX A1+sumXXXX A2 + sumXXXXX A3+  sumXXXXXX A4 =sumXXXY
    LaneParam param;
	long double sumX = 0, sumY = 0, sumXX = 0, sumXXX = 0, sumXXXX = 0, sumXXXXX = 0, sumXXXXXX = 0, sumXXXXXXX = 0, sumXY = 0, sumXXY = 0, sumXXXY = 0;//建立方程组
	int points_num = points.size();
	int data_n = points.size();
    int n = points_num;
    for (int i = 0; i < points_num; i++)
	{
		// cout << "number:" << points[i].second << endl;
		// cout << "gray:" << points[i].first << endl;
		sumX = sumX + points[i].first;
		sumY = sumY + points[i].second;
		sumXX = sumXX + points[i].first*points[i].first;
		sumXXX = sumXXX + points[i].first*points[i].first*points[i].first;
		sumXXXX = sumXXXX + points[i].first*points[i].first*points[i].first*points[i].first;
		sumXXXXX = sumXXXXX + points[i].first*points[i].first*points[i].first*points[i].first*points[i].first;//
		sumXXXXXX = sumXXXXXX + points[i].first*points[i].first*points[i].first*points[i].first*points[i].first*points[i].first;
		//	sumXXXXXXX = sumXXXXXXX + points[i].first*points[i].first*points[i].first*points[i].first*points[i].first*points[i].first*points[i].first;
		sumXY = sumXY + points[i].first*points[i].second;
		sumXXY = sumXXY + points[i].first*points[i].second*points[i].first;
		sumXXXY = sumXXXY + points[i].first*points[i].second*points[i].first*points[i].first;
	}
	// cout << "n:" << n << " sumX: " << sumX << " sumXX: " << sumXX << " sumXXX: " << sumXXX << " sumXXXX: " << sumXXXX << " sumXXXXX: " << sumXXXXX << " sumXXXXXX: " << sumXXXXXX << " sumXXXXXXX: " << sumXXXXXXX << endl;
	// cout << " sumY: " << sumY << " sumXY: " << sumXY << " sumXXY: " << sumXXY << " sumXXXY: " << sumXXXY << endl;
 
	double  a11 = n;      double  a12 = sumX;    double  a13 = sumXX;    double  a14 = sumXXX;    double  b1 = sumY;      //构造矩阵   求解方程系数   a11 第一行第一列
	double  a21 = sumX;   double  a22 = sumXX;   double  a23 = sumXXX;   double  a24 = sumXXXX;   double  b2 = sumXY;
	double  a31 = sumXX;  double  a32 = sumXXX;  double  a33 = sumXXXX;  double  a34 = sumXXXXX;  double  b3 = sumXXY;
	double  a41 = sumXXX; double  a42 = sumXXXX; double  a43 = sumXXXXX; double  a44 = sumXXXXXX; double  b4 = sumXXXY;
	//double a11 = 2;      double  a12 = 4;   double  a13 = -2;   double  b1 = 2;      //构造矩阵   求解方程系数   a11 第一行第一列
	//double  a21 = 1;  double  a22 = -3;  double  a23 = -3;  double  b2 = -1;
	//double  a31 = 4; double  a32 = 2; double  a33 = 2; double  b3 = 3;
 
	//R2=R2-(a21/a11)*R1     ------------------------------------------------------------ - 进行消元    高斯消元法  第一例消元
	double a21x, a22x, a23x, a24x, b2x;
	a21x = a21 - a21 / a11*a11; a22x = a22 - a21 / a11*a12; a23x = a23 - a21 / a11*a13; a24x = a24 - a21 / a11*a14; b2x = b2 - a21 / a11*b1;
	//R3=R3-(a31/a11)*R1
	double a31x, a32x, a33x, a34x, b3x;
	a31x = a31 - a31 / a11*a11; a32x = a32 - a31 / a11*a12; a33x = a33 - a31 / a11*a13; a34x = a34 - a31 / a11*a14; b3x = b3 - a31 / a11*b1;
	//R4=R4-(a41/a11)*R1
	double a41x, a42x, a43x, a44x, b4x;
	a41x = a41 - a41 / a11*a11; a42x = a42 - a41 / a11*a12; a43x = a43 - a41 / a11*a13; a44x = a44 - a41 / a11*a14; b4x = b4 - a41 / a11*b1;
 
	//------------------------------------------------------------ - 进行消元    高斯消元法  第二例消元
	//R3=R3-(a32/a22)*R2
	double a32xx, a33xx, a34xx, b3xx;
	a32xx = a32x - a32x / a22x*a22x; a33xx = a33x - a32x / a22x*a23x; a34xx = a34x - a32x / a22x*a24x; b3xx = b3x - a32x / a22x*b2x;
	//R4=R4-(a42/a22)*R2
	double a42xx, a43xx, a44xx, b4xx;
	a42xx = a42x - a42x / a22x*a22x; a43xx = a43x - a42x / a22x*a23x; a44xx = a44x - a42x / a22x*a24x; b4xx = b4x - a42x / a22x*b2x;
 
	//------------------------------------------------------------ - 进行消元    高斯消元法  第三例消元
	//R4=R4-(a43/a33)R3
	double a43xxx, a44xxx, b4xxx;
	a43xxx = a43xx - a43xx / a33xx*a33xx; a44xxx = a44xx - a43xx / a33xx*a34xx; b4xxx = b4xx - a43xx / a33xx*b3xx;
 
	//------------------------------------------------------------求得系数矩阵未知数
	param.p4 = b4xxx / a44xxx;
	param.p3 = (b3xx - a34xx*param.p4) / a33xx;
	param.p2 = (b2x - a23x*param.p3 - a24x*param.p4) / a22x;
	param.p1 = (b1 - a12*param.p2 - a13*param.p3 - a14*param.p4) / a11;
	param.range = points[data_n-1].first; 
    return param;
}

/// y = p1+p2*x + p3*x*x  y 侧向方向  x 车头方向  
LaneParam CalcPolfit2(std::vector<std::pair<double,double>> points)
{

	// int jiange = 10;//相隔十个点选一个点进行计算
	// n = n / jiange + 1;
	//-----------------公式---------------------------
	//n A1     + sumX A2   + sumXX A3      = sumY
	//sumX A1  + sumXX A2  + sumXXX A3     = sumXY
	//sumXX A1 + sumXXX A2 + sumXXXX A3    = sumXXY
    LaneParam param;
	long double sumX = 0, sumY = 0, sumXX = 0, sumXXX = 0, sumXXXX = 0, sumXY = 0, sumXXY = 0, sumXXXY = 0;//建立方程组
	int points_num = points.size();
	int data_n = points.size();
    int n = points_num;
    for (int i = 0; i < points_num; i++)
	{
		// cout << "number:" << points[i].second << endl;
		// cout << "gray:" << points[i].first << endl;
		sumX = sumX + points[i].first;
		sumY = sumY + points[i].second;
		sumXX = sumXX + points[i].first*points[i].first;
		sumXXX = sumXXX + points[i].first*points[i].first*points[i].first;
		sumXXXX = sumXXXX + points[i].first*points[i].first*points[i].first*points[i].first;
		sumXY = sumXY + points[i].first*points[i].second;
		sumXXY = sumXXY + points[i].first*points[i].second*points[i].first;
		sumXXXY = sumXXXY + points[i].first*points[i].second*points[i].first*points[i].first;
	}
	// cout << "n:" << n << " sumX: " << sumX << " sumXX: " << sumXX << " sumXXX: " << sumXXX << " sumXXXX: " << sumXXXX << " sumXXXXX: " << sumXXXXX << " sumXXXXXX: " << sumXXXXXX << " sumXXXXXXX: " << sumXXXXXXX << endl;
	// cout << " sumY: " << sumY << " sumXY: " << sumXY << " sumXXY: " << sumXXY << " sumXXXY: " << sumXXXY << endl;
 
	double  a11 = n;      double  a12 = sumX;    double  a13 = sumXX;      double  b1 = sumY;      //构造矩阵   求解方程系数   a11 第一行第一列
	double  a21 = sumX;   double  a22 = sumXX;   double  a23 = sumXXX;     double  b2 = sumXY;
	double  a31 = sumXX;  double  a32 = sumXXX;  double  a33 = sumXXXX;    double  b3 = sumXXY;
	//double a11 = 2;      double  a12 = 4;   double  a13 = -2;   double  b1 = 2;      //构造矩阵   求解方程系数   a11 第一行第一列
	//double  a21 = 1;  double  a22 = -3;  double  a23 = -3;  double  b2 = -1;
	//double  a31 = 4; double  a32 = 2; double  a33 = 2; double  b3 = 3;
 
	//R2=R2-(a21/a11)*R1     ------------------------------------------------------------ - 进行消元    高斯消元法  第一例消元
	double a21x, a22x, a23x, b2x;
	a21x = a21 - a21 / a11*a11; a22x = a22 - a21 / a11*a12; a23x = a23 - a21 / a11*a13; b2x = b2 - a21 / a11*b1;
	//R3=R3-(a31/a11)*R1
	double a31x, a32x, a33x, b3x;
	a31x = a31 - a31 / a11*a11; a32x = a32 - a31 / a11*a12; a33x = a33 - a31 / a11*a13; b3x = b3 - a31 / a11*b1;
 
	//------------------------------------------------------------ - 进行消元    高斯消元法  第二例消元
	//R3=R3-(a32/a22)*R2
	double a32xx, a33xx, b3xx;
	a32xx = a32x - a32x / a22x*a22x; a33xx = a33x - a32x / a22x*a23x; b3xx = b3x - a32x / a22x*b2x;
 
	//------------------------------------------------------------求得系数矩阵未知数
	param.p4 = 0;
	param.p3 = (b3xx) / a33xx;
	param.p2 = (b2x - a23x*param.p3) / a22x;
	param.p1 = (b1 - a12*param.p2 - a13*param.p3) / a11;
	param.range = points[data_n-1].first; 
    return param;
}


/************************************************************************* 
 最小二乘法拟合直线，y = a*x + b; n组数据; r-相关系数[-1,1],fabs(r)->1,说明x,y之间线性关系好，fabs(r)->0，x,y之间无线性关系，拟合无意义 
 a = (n*C - B*D) / (n*A - B*B) 
 b = (A*D - B*C) / (n*A - B*B) 
 r = E / F 
 其中： 
 A = sum(Xi * Xi) 
 B = sum(Xi) 
 C = sum(Xi * Yi) 
 D = sum(Yi) 
 E = sum((Xi - Xmean)*(Yi - Ymean)) 
 F = sqrt(sum((Xi - Xmean)*(Xi - Xmean))) * sqrt(sum((Yi - Ymean)*(Yi - Ymean))) 
 
**************************************************************************/  
LaneParam CalcPolfit(std::vector<std::pair<double,double>> points)
{
	LaneParam param;


	double t1=0, t2=0, t3=0, t4=0;  
	int data_n = points.size();
	for(int i=0; i<points.size(); ++i)  
	{  
		t1 += points[i].first*points[i].first;  
		t2 += points[i].first;  
		t3 += points[i].first*points[i].second;  
		t4 += points[i].second;  
	}  
	param.p2 = (t3*points.size() - t2*t4) / (t1*points.size() - t2*t2);  
	//b = (t4 - a*t2) / x.size();  
	param.p1 = (t1*t4 - t2*t3) / (t1*points.size() - t2*t2); 
	param.p3 = 0;
	param.p4 = 0;
	// param.range = sqrt(points[data_n-1].first * points[data_n-1].first + param.p2*points[data_n-1].first * param.p2*points[data_n-1].first); 
	// param.range = points[data_n-1].first; 
	param.range = data_n; 
	return param;
}
